#include "alglsyst.h"
#include <iostream>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
using std::cout;
using std::endl;

AlgebraicLinearSystem::AlgebraicLinearSystem(){}
AlgebraicLinearSystem::AlgebraicLinearSystem(const vector<vector<double> >&  _factorMatrix,const vector<double>&  _rightPart):
    factorMatrix(_factorMatrix),rightPart(_rightPart) {}

void PrintSystem(const AlgebraicLinearSystem& aSyst)
{
    for(unsigned int i=0;i<aSyst.factorMatrix.size();i++)
    {
        for(unsigned int u=0;u<aSyst.factorMatrix.size();u++)
        {
            printf("%f ",aSyst.factorMatrix[i][u]);
        }
        printf("|%f \n",aSyst.rightPart[i]);
    }
}

vector<double> FullVectorOnFunc(double (*fulling_func)(double),double start,double end,double step)
{
    vector<double> res((unsigned int)((end-start)/step+1));
    double arg=start;
    for(unsigned int i=0;i<res.size();i++)
    {
        (*(res.begin()+i))=(*fulling_func)(arg);
        arg+=step;
    }
    return res;
}

vector<double>  GaussMeth(const AlgebraicLinearSystem& aSyst)
{

    vector<vector<double> > lmatr(aSyst.factorMatrix);

    for(unsigned int i=0;i<lmatr.size();i++)
        for(unsigned int u=0;u<lmatr[i].size();u++)
            if(fabs(lmatr[i][u])<0.00000001)
                lmatr[i][u]=0;
    vector<double> lrpart(aSyst.rightPart);
    vector<double> res(lrpart.size(),0);
    for(unsigned int lin=0;lin<(lmatr.size()-1);lin++)
    {
        bool goodmatrix=false;
        if(lmatr[lin][lin]==0)
        {
            unsigned int i=lin+1;
            for(;i<lmatr.size();i++)
                if(lmatr[i][lin]!=0)
                {
                    goodmatrix=true;
                    break;
                }
            if(!goodmatrix)
            {
                cout<<"GaussMeth:Bad matrix";
                exit(1);
            }
            double swap=0;
            for(unsigned int col=lin;col<lmatr.size();col++)
            {
                swap=lmatr[i][col];
                *(lmatr[i].begin()+col)=lmatr[lin][col];
                *(lmatr[lin].begin()+col)=swap;
            }
            swap=lrpart[i];
            *(lrpart.begin()+i)=lrpart[lin];
            *(lrpart.begin()+lin)=swap;
        }
        for(unsigned int lin2=lin+1;lin2<lmatr.size();lin2++)
        {
            double koeff=-lmatr[lin2][lin]/lmatr[lin][lin];
            for(unsigned int col=lin;col<lmatr.size();col++)
                *(lmatr[lin2].begin()+col)=lmatr[lin2][col]+koeff*lmatr[lin][col];
            *(lrpart.begin()+lin2)=lrpart[lin2]+koeff*lrpart[lin];
        }
    }

    for(int lin=lmatr.size()-1;lin>=0;lin--)
    {
        double tres=lrpart[lin];
        for(unsigned int col=lmatr.size()-1;col>lin;col--)
            tres-=lmatr[lin][col]*res[col];
        tres/=lmatr[lin][lin];
        *(res.begin()+lin)=tres;
    }
    return res;
}

void PrintMatr(const vector<vector<double> >& matr)
{
    for(unsigned int i=0;i<matr.size();i++)
    {
        for(unsigned int u=0;u<matr.size();u++)
            printf("%f ",matr[i][u]);
        cout<<endl;
    }
}

vector<double> SweepMeth(const AlgebraicLinearSystem& aSyst)
{
    bool correct_sweep_matrix=true;
    for(unsigned int i=0;i<aSyst.factorMatrix.size();i++)
    {
        for(unsigned int u=0;u<aSyst.factorMatrix.size();u++)
            if(((int)u<(int)(i-1)||u>i+1)&&aSyst.factorMatrix[i][u]!=0)
            {
                correct_sweep_matrix=false;
                break;
            }
        if(!correct_sweep_matrix)
        {
            break;
        }
    }
    if(!correct_sweep_matrix)
    {
        cout<<"Error: Not correct sweep matrix"<<endl;
        return aSyst.rightPart;
    }
    vector<double> AVec(aSyst.factorMatrix.size());
    vector<double> BVec(aSyst.factorMatrix.size());
    AVec[0]=-aSyst.factorMatrix[0][1]/aSyst.factorMatrix[0][0];
    BVec[0]=aSyst.rightPart[0]/aSyst.factorMatrix[0][0];
    for(unsigned int i=1;i<(aSyst.factorMatrix.size()-1);i++)
    {
       AVec[i]=-aSyst.factorMatrix[i][i+1]/(aSyst.factorMatrix[i][i]+aSyst.factorMatrix[i][i-1]*AVec[i-1]);
       BVec[i]=(aSyst.rightPart[i]-aSyst.factorMatrix[i][i-1]*BVec[i-1])/(aSyst.factorMatrix[i][i]+aSyst.factorMatrix[i][i-1]*AVec[i-1]);
    }
    AVec[aSyst.factorMatrix.size()-1]=0;
    BVec[aSyst.factorMatrix.size()-1]=(aSyst.rightPart[aSyst.factorMatrix.size()-1]-aSyst.factorMatrix[aSyst.factorMatrix.size()-1][aSyst.factorMatrix.size()-2]*BVec[aSyst.factorMatrix.size()-2])/(aSyst.factorMatrix[aSyst.factorMatrix.size()-1][aSyst.factorMatrix.size()-1]+aSyst.factorMatrix[aSyst.factorMatrix.size()-1][aSyst.factorMatrix.size()-2]*AVec[aSyst.factorMatrix.size()-2]);

    vector<double> res(aSyst.factorMatrix.size());
    res[aSyst.factorMatrix.size()-1]=BVec[aSyst.factorMatrix.size()-1];
    for(int i=(int)(aSyst.factorMatrix.size()-2);i>=0;i--)
        res[i]=AVec[i]*res[i+1]+BVec[i];
    return res;
}

void PrintVec(const vector<double>& rpart)
{
    for(unsigned int i=0;i<rpart.size();i++)
    {
        printf("%f ",rpart[i]);
    }
    cout<<endl;
}


/*void SpinMatr(vector<vector<double> >& matr,vector<double>& rpart)
{
    vector<vector<double> > bmatr(matr);
    vector<double> cvec(rpart);
    for(int i=0;i<matr.size();i++)
    {
        for(int u=0;u<matr.size();u++)
        {
            (*(bmatr[i].begin()+u))=(i==u)?0:((-matr[i][u])/matr[i][i]);
        }
        (*(cvec.begin()+i))=rpart[i]/matr[i][i];
    }
    matr=bmatr;
    rpart=cvec;
}

void DownTrialMatr(vector<vector<double> >& matr)
{
    for(int i=0;i<matr.size();i++)
    {
        for(int u=i;u<matr.size();u++)
        {
            (*(matr[i].begin()+u))=0;
        }
    }
}

void UpTrialMatr(vector<vector<double> >& matr)
{
    for(int i=0;i<matr.size();i++)
    {
        for(int u=0;u<i;u++)
        {
            (*(matr[i].begin()+u))=0;
        }
    }
}


*/

